global spacenum f1 f4 result bound
% f1=figure;
% f4=figure;
spacenum=16;
bound=[zeros(spacenum,1) ones(spacenum,1)];

% cacu_result(8,linspace(0,1,spacenum),@fitnessf);
[x_result,f_result,cost_time]=trajectory_planningf_BSO(linspace(0,1,spacenum),@fitnessf,@paraplot3);
% [x_result,f_result,cost_time]=trajectory_planningf_PSO(linspace(0,1,spacenum),@fitnessf,@paraplot3);
% [x_result,f_result,cost_time]=trajectory_planningf_GA(linspace(0,1,spacenum),@fitnessf,@paraplot3)

function a=fitnessf(pop,para)
    [pops,numvar]=size(pop);
    a=zeros(1,pops);
    for i=1:pops
        diff_x=diff(pop(i,:));
        diff_y=diff(para);
        fq=sqrt(diff_x.^2+diff_y.^2);
        fit=sum(fq);
        a(i)=1./fit;
    end
end

function moveplot(para,sol,iter,n)
    global spacenum f1 f4
    poo=[sol;para];
    pcart=poo';
    
    xi=linspace(0,1,spacenum*2);
    figure(f1),
    y1=poo(1,:);
    plot(xi,y1,'.','color','r','MarkerSize',20);hold off;
    
    px=pcart(:,1);py=pcart(:,2);
%     pcart2=forkin(poo(1:2,:));
%     px2=pcart2(1,:);py2=pcart2(2,:);
    figure(f4),
    for i=1:size(px,1)
        plot(px(1:i),py(1:i))
        hold on
    %     plot(px2(1:i),py2(1:i))
    %     hold on
        axis([-4 4 -4 4])
        text(0,2.6,['t=',num2str(i/size(px,1))])
        hold on
        text(0,3,['gen.',num2str(iter)])
        hold off
        pause(0)
    end
end

function paraplot3(n,maxf,kk,bsol,rec)
    e=[1:n];
    figure,plot(e,1./maxf)
    xlabel('generation')
    ylabel('min. fitness')
end